/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package dca.sistemascontrole.malhadecontrole;

import dca.sistemascontrole.domain.VariaveisDeControle;
import dca.sistemascontrole.util.ControleConstantes;

/**
 *
 * @author Rafael
 */
public class ControladorPD extends MalhaFechada {

    private double up;
    private double ud;
    private double erroAnteriorPD;
    private double udAnteriorPD;

    public ControladorPD(double setpoint, double kp, double kd, boolean usarFiltroDerivativo) {
        super(setpoint);
        this.kp = kp;
        this.kd = kd;
        this.usarFiltroDerivativo = usarFiltroDerivativo;
        erroAnteriorPD = Double.MIN_VALUE;
        udAnteriorPD = Double.MIN_VALUE;
    }

    @Override
    public int getTipo() {
        return PD;
    }

    @Override
    protected VariaveisDeControle getNextValue(long t, double nivelTanqueSuperior, double nivelTanqueInfeior) {
        double erro;
        double U;
        double pv;

        if (ControleConstantes.PV_IS_TANQUE_SUPERIOR) {
            pv = nivelTanqueSuperior;
        } else {
            pv = nivelTanqueInfeior;
        }

        // U inicializado com o peso proporcional do P
        erro = variaveisDeControle.getSetPoint() - pv;
        if (erroAnteriorPD != Double.MIN_VALUE) {
            if (usarFiltroDerivativo) {
//            ud = (kd * (erro - erroAnteriorPD))/((kd*gama*(erro - erroAnteriorPD))+1);
                ud = ((kd * (erro - erroAnteriorPD)) + (udAnteriorPD * kd * gama)) / (0.1 + kd * gama);
            } else {
                ud = kd * (erro - erroAnteriorPD);
            }
        } else {
            ud = 0;
        }

        up = kp * erro;
        U = up + ud;
        erroAnteriorPD = erro;

        variaveisDeControle.setU(U);
        variaveisDeControle.setUp(up);
        variaveisDeControle.setUd(ud);
        variaveisDeControle.setErro(erro);

        return variaveisDeControle;
    }
}
